/**
 * 计算工具
 */

#include <math.h>
#include "flight_tool.h"

FlightTool* FlightTool::instance = nullptr;

float FlightTool::GetDistance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2)
{
    float dx = p1.pose.position.x - p2.pose.position.x;
    float dy = p1.pose.position.y - p2.pose.position.y;
    float dz = p1.pose.position.z - p2.pose.position.z;
    return sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2));
}

float FlightTool::GetDistance(const geographic_msgs::GeoPoseStamped &p1, const geographic_msgs::GeoPoseStamped &p2)
{
    float dx = p1.pose.position.latitude - p2.pose.position.latitude;
    float dy = p1.pose.position.longitude - p2.pose.position.longitude;
    float dz = p1.pose.position.altitude - p2.pose.position.altitude;
    return sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2));
}